#include "../include/param.h"
#include <opencv2/core/eigen.hpp>
cv::Mat Param::BASE2CAM;
cv::Mat Param::KK;
cv::Mat Param::DISTORT;
bool Param::ENABLE_DEBUG;

 //行人检测相关
std::string Param::DETECT_MODEL_LOCATION;//行人检测模型路径
std::string Param::SOT_MODEL_LOCATION;//SOT模型路径


//圆柱相关
Eigen::Vector2f Param::LEFT_COLUMN_COORDS;//左侧圆柱坐标
int Param::LC_H_MIN;//HSV H最小值
int Param::LC_H_MAX;//HSV H最大值
int Param::LC_S_MIN;//HSV V最小值
int Param::LC_S_MAX;//HSV V最大值
int Param::LC_V_MIN;//HSV V最小值
int Param::LC_V_MAX;//HSV V最大值

Eigen::Vector2f Param::RIGHT_COLUMN_COORDS;//左侧圆柱坐标
int Param::RC_H_MIN;//HSV H最小值
int Param::RC_H_MAX;//HSV H最大值
int Param::RC_S_MIN;//HSV V最小值
int Param::RC_S_MAX;//HSV V最大值
int Param::RC_V_MIN;//HSV V最小值
int Param::RC_V_MAX;//HSV V最大值

//定位相关
cv::Mat Param::COORDS_MEASURE_COV;//坐标测量噪声协方差
cv::Mat Param::COORDS_MODEL_COV;//坐标模型噪声协方差
cv::Mat Param::YAW_MODEL_COV;//yaw模型噪声协方差
cv::Mat Param::YAW_MEASURE_COV;//yaw测量噪声协方差

void Param::loadParam(const std::string &yaml_path) {
    cv::FileStorage fs;
    fs.open(yaml_path, cv::FileStorage::READ);
    if (!fs.isOpened()) {
        std::cerr << "Open startParam file in Param.cpp failed!" << std::endl;
        std::cerr << "filename:" << yaml_path << std::endl;
        exit(1);
    }
    cv::FileNode node_cam = fs["Camera"];
    cv::FileNode node_detect = fs["Detect"];
    cv::FileNode node_localize = fs["Localization"];

    //加载相机参数
    node_cam["camera_matrix"] >> Param::KK;
    node_cam["distortion_coefficients"] >> Param::DISTORT;
    cv::Mat r_vec, r_mat, t_vec;
    node_cam["t_vec"] >> t_vec;
    node_cam["r_vec"] >> r_vec;

    cv::Rodrigues(r_vec, r_mat);
    r_mat = r_mat.inv();
    Param::BASE2CAM = cv::Mat::zeros(cv::Size(4, 4), CV_64FC1);
    ///通过平移矩阵和旋转矩阵得到云台坐标系相对于相机坐标系的转移矩阵(齐次坐标系)
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
        Param::BASE2CAM.at<double>(i, j) = r_mat.at<double>(i, j);
        }
    }
    Param::BASE2CAM.at<double>(0, 3) = t_vec.at<double>(0, 0);
    Param::BASE2CAM.at<double>(1, 3) = t_vec.at<double>(0, 1);
    Param::BASE2CAM.at<double>(2, 3) = t_vec.at<double>(0, 2);
    Param::BASE2CAM.at<double>(3, 3) = 1;

    //加载检测参数
    node_detect["ENABLE_DEBUG"]>>Param::ENABLE_DEBUG;
    
    node_detect["SOT_MODEL_LOCATION"] >> Param::SOT_MODEL_LOCATION;
    node_detect["DETECT_MODEL_LOCATION"] >> Param::DETECT_MODEL_LOCATION;

    //圆柱检测
    cv::Mat l_coords,r_coords;
    node_detect["LEFT_COLUMN_COORDS"]>>l_coords;
    cv::cv2eigen(l_coords,Param::LEFT_COLUMN_COORDS);
    node_detect["LC_H_MIN"]>>Param::LC_H_MIN;
    node_detect["LC_H_MAX"]>>Param::LC_H_MAX;
    node_detect["LC_S_MIN"]>>Param::LC_S_MIN;
    node_detect["LC_S_MAX"]>>Param::LC_S_MAX;
    node_detect["LC_V_MIN"]>>Param::LC_V_MIN;
    node_detect["LC_V_MAX"]>>Param::LC_V_MAX;

    node_detect["RIGHT_COLUMN_COORDS"]>>r_coords;
    cv::cv2eigen(r_coords,Param::RIGHT_COLUMN_COORDS);
    node_detect["RC_H_MIN"]>>Param::RC_H_MIN;
    node_detect["RC_H_MAX"]>>Param::RC_H_MAX;
    node_detect["RC_S_MIN"]>>Param::RC_S_MIN;
    node_detect["RC_S_MAX"]>>Param::RC_S_MAX;
    node_detect["RC_V_MIN"]>>Param::RC_V_MIN;
    node_detect["RC_V_MAX"]>>Param::RC_V_MAX;

    //定位
    node_localize["COORDS_MEASURE_COV"]>>Param::COORDS_MEASURE_COV;
    node_localize["COORDS_MODEL_COV"]>>Param::COORDS_MODEL_COV;
    node_localize["YAW_MODEL_COV"]>>Param::YAW_MODEL_COV;
    node_localize["YAW_MEASURE_COV"]>>Param::YAW_MEASURE_COV;

    std::cout << "[Param] load config file:" << yaml_path << std::endl;
    std::cout << "[Param] get tracking model name: " << Param::SOT_MODEL_LOCATION << std::endl;
    fs.release();
}

void Param::print_debug_string()
{
    std::cout<<"[PARAM FILE]:"<<std::endl;
    std::cout<<"\t left column world coords:"<<Param::LEFT_COLUMN_COORDS.transpose()<<std::endl;
    std::cout<<"\t right column world coords:"<<Param::RIGHT_COLUMN_COORDS.transpose()<<std::endl;
    std::cout<<"\t left column h min:"<<Param::LC_H_MIN<<std::endl;
    std::cout<<"\t left column h max:"<<Param::LC_H_MAX<<std::endl;
    std::cout<<"\t left column s min:"<<Param::LC_S_MIN<<std::endl;
    std::cout<<"\t left column s max:"<<Param::LC_S_MAX<<std::endl;
    std::cout<<"\t left column v min:"<<Param::LC_V_MIN<<std::endl;
    std::cout<<"\t left column v max:"<<Param::LC_V_MAX<<std::endl;

    std::cout<<"\t right column h min:"<<Param::RC_H_MIN<<std::endl;
    std::cout<<"\t right column h max:"<<Param::RC_H_MAX<<std::endl;
    std::cout<<"\t right column s min:"<<Param::RC_S_MIN<<std::endl;
    std::cout<<"\t right column s max:"<<Param::RC_S_MAX<<std::endl;
    std::cout<<"\t right column v min:"<<Param::RC_V_MIN<<std::endl;
    std::cout<<"\t right column v max:"<<Param::RC_V_MAX<<std::endl;
}
